#include "pole_vision.h"


using namespace std;
using namespace cv;
using namespace Eigen;

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
	double t = (double)cv::getTickCount();
	//double total = (double)cv::getTickCount();
	double track_t_begin,total_begin_t;
	boost::mutex::scoped_lock lock(frame_grab_mutex);
 	int j;

	sensor_msgs::CvBridge bridge;

	/* Get the first frame and create a edges image with the same size */
	try
	{
		frame = bridge.imgMsgToCv(msg, "bgr8");
	}
	catch (sensor_msgs::CvBridgeException& e)
	{
		ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
	}

	grey  = cvCreateImage(cvGetSize(frame), 8, 1);
	canny_edges = cvCreateImage(cvGetSize(frame), 8, 1);

	#ifdef DRAW
	draw_img = cvCreateImage( cvSize(IMG_WIDTH,IMG_HEIGHT), 8, 3 );
	#endif
	
	/* Edges on the input gray image (needs to be grayscale) using the Canny algorithm.
	Uses two threshold and a aperture parameter for Sobel operator. */
	cvCvtColor(frame, grey, CV_BGR2GRAY);
	
	
	
    //////////////////////////////////////////////////
    //
	//   Boot strapping using canny and hough transform.
    //   must find two pole lines in vline vector.
	//////////////////////////////////////////////////
	if(!initialized)
	{
		
			
			
			
			//cvShowImage("view", grey);



			if( (nAperture & 1) == 0 ) nAperture +=1;
			if( nAperture < 3) nAperture=3;
			if( nAperture > 7 ) nAperture=7;

		#ifdef CHECK_DURA
			double pre = (((double)cv::getTickCount() - t)/cv::getTickFrequency())*1000;

			ROS_INFO("Preperation duration:\t %.3f ms", pre);
		#endif


		#ifdef CHECK_DURA
			t = (double)cv::getTickCount();
		#endif

			cvCanny( grey, canny_edges, nThreshold1, nThreshold2, nAperture);

		#ifdef CHECK_DURA
			double canny_t = (((double)cv::getTickCount() - t)/cv::getTickFrequency())*1000;

			ROS_INFO("Canny duration:\t %.3f ms", canny_t);
		#endif
		
		#ifdef DRAW
			cvCvtColor( canny_edges, draw_img, CV_GRAY2BGR );
		#endif
			if( nthreshold_hough <= 0) nthreshold_hough=1;
			if( nparam1_hough <= 0 ) nparam1_hough=1;
			if( nparam2_hough <= 0 ) nparam1_hough=1;

		#ifdef CHECK_DURA
			t = (double)cv::getTickCount();
		#endif

			lines = cvHoughLines2( canny_edges,
			storage,
			CV_HOUGH_PROBABILISTIC,
			1,
			CV_PI/180,
			nthreshold_hough,
			nparam1_hough,
			nparam2_hough );

		#ifdef CHECK_DURA
			double hough_t = (((double)cv::getTickCount() - t)/cv::getTickFrequency())*1000;

			ROS_INFO("Hough transform duration:\t %.3f ms", hough_t);
		#endif


		#ifdef CHECK_DURA
			t = (double)cv::getTickCount();
		#endif

			// will print out Inf until sec is greater than 0
			//printf("FPS = %.2f\n", fps);
			//cout<<"# lines = "<<lines->total<<endl;
		
			for( i = 0; i < lines->total; i++ )
	        {
	            CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i);
	            //cvLine( color_dst, line[0], line[1], CV_RGB(255,255,0), 1, 8 );
				//cout<<"pt0["<<j<<"].x="<<line[0].x<<" pt0["<<j<<"].y="<<line[0].y<<endl;
				//cout<<"pt1["<<j<<"].x="<<line[1].x<<" pt1["<<j<<"].y="<<line[1].y<<endl;
				vline_length.push_back(hypot(line[0].x-line[1].x,line[0].y-line[1].y));

				if( (line[0].x >100) && (line[0].x<220) && (line[1].x >100) && (line[1].x<220) && vline_length[i] >120)
				{
					//cvLine( color_dst, line[0], line[1], CV_RGB(255,0,0), 2, 8 );
					line_t line_temp;
					line_temp.pt0=line[0];
					line_temp.pt1=line[1];
					vline.push_back(line_temp);
				}
	        }

	#if 1
			cout<<"vline.size()="<<vline.size()<<endl;
			for(int i =0;i<vline.size();i++) cout<<"pt0.x="<<vline[i].pt0.x<<
												  " pt0.y="<<vline[i].pt0.y<<
												  " pt1.x="<<vline[i].pt1.x<<
												  " pt1.y="<<vline[i].pt1.y<<endl;
	#endif

			double distance_temp;
			double m0_temp,m1_temp;
			double c0_temp,c1_temp;
			if(vline.size()==2)
			{
				//Validate candidates
				//Firstly, pick up two lines from candidates.
				line_t line_temp0 = vline[0];
				line_t line_temp1 = vline[1];

				distance_temp = getDistance_Pt_Line(line_temp0.pt0,line_temp1.pt0,line_temp1.pt1);
 	 		  	cout<<"distance two lines from bootstrap= "<<distance_temp<<endl;

				
				m0_temp = (double)atan2(vline[0].pt1.x-vline[0].pt0.x,vline[0].pt1.y-vline[0].pt0.y);
				m1_temp = (double)atan2(vline[1].pt1.x-vline[1].pt0.x,vline[1].pt1.y-vline[1].pt0.y);
				cout<<"two lines, angle btw two lines from bootst= "<<RAD_TO_DEG(m0_temp-m1_temp)<<"(deg)"<<endl;


				//if( ( distance_temp<30) || abs(RAD_TO_DEG(m0_temp-m1_temp))>30)
				if( ( distance_temp<30) )
				{
					vline.clear();
				}
				else
				{
					#if 0
					for(int i =0;i<vline.size();i++) cout<<"pt0.x="<<vline[i].pt0.x<<
												  " pt0.y="<<vline[i].pt0.y<<
												  " pt1.x="<<vline[i].pt1.x<<
												  " pt1.y="<<vline[i].pt1.y<<"from two"<<endl;
					#endif
					initialized=1;

					// Need to send the initial pose to EKF
	
					cout<<"Here 1"<<endl;
					float pole_diameter_inpx = distance_temp;
					//cout<<"prev = "<<pole_diameter_inpx<<" now = "<<pole_diameter_inpx_<<endl;

					//float pole_center = (out0.c+out1.c)/2;
					float pole_center = (vline[0].pt1.y+vline[1].pt1.y)/2;
					cout<<"line 0 y = "<<vline[0].pt1.y<<"line 1 y = "<<vline[1].pt1.y<<endl;
					//cout<<"prev ="<<pole_center<<" now = "<<pole_center_<<endl; 
					float init_x = (float)((focal_length*pole_diameter)/(pixel_size*pole_diameter_inpx));
					float init_y = (float)((pole_center-160)*pixel_size*init_x/focal_length);
					cout<<"Init x = "<<init_x<<endl;
					cout<<"Init y = "<<init_y<<endl;
					msg_pole_init.data.push_back(init_x);
					msg_pole_init.data.push_back(init_y);
					//ros::WallDuration(1).sleep();
					m_pose_init_pub.publish(msg_pole_init);
					//cout<<"Publishing initial pose"<<endl;
					//ros::WallDuration(1).sleep();		
					//cout<<"Publishing initial pose2"<<endl;			
 					//for(int i=0;i<10;i++) m_pose_init_pub.publish(msg_pole_init);
					//msg_pole_init.data.clear();
					//ros::WallDuration(0.5).sleep();

					Vector2f pt0,pt1,pt2,pt3;
					pt0<<vline[0].pt0.x,vline[0].pt0.y,1;
					pt1<<vline[0].pt1.x,vline[0].pt1.y,1;
					pt2<<vline[1].pt0.x,vline[1].pt0.y,1;
					pt3<<vline[1].pt1.x,vline[1].pt1.y,1;

					cout<<"pt0 = "<<pt0<<"pt1 = "<<pt1<<endl;
					cout<<"pt2 = "<<pt2<<"pt3 = "<<pt3<<endl;

					// Swap pt0,pt1 to pt2,pt3.
					if(pt0(0) > pt2(0))
					{
						Vector2f temp;
						temp = pt0;
						pt0 = pt2;
						pt2 = temp;
						temp = pt1;
						pt1 = pt3;
						pt3 = temp;
					} 

	
					boot_homline0=homline(pt0,pt1);
					boot_homline1=homline(pt2,pt3);
					//cout<<boot_homline0<<endl;
					//cout<<boot_homline1<<endl;


					


				}
			}

			else if(vline.size()>2)
			{
				//Should do some smart filtering to pick up two pole detected lines but next time.
				//Pick the first element as the detected line, based on that trying to get rid of similar
				//lines of it
				line_t line_base= vline[0];
				for(int i =0;i<vline.size()-1;i++)
				{
					if(vline.size()==2)
					{
						//Validate candidates
						//Firstly, pick up two lines from candidates.
						line_t line_temp0 = vline[0];
						line_t line_temp1 = vline[1];

						distance_temp = getDistance_Pt_Line(line_temp0.pt0,line_temp1.pt0,line_temp1.pt1);
						cout<<"distance two lines from bootstrap= "<<distance_temp<<endl;


						m0_temp = (double)atan2(vline[0].pt1.x-vline[0].pt0.x,vline[0].pt1.y-vline[0].pt0.y);
						m1_temp = (double)atan2(vline[1].pt1.x-vline[1].pt0.x,vline[1].pt1.y-vline[1].pt0.y);
						cout<<"more than 2 lines, angle btw two lines from bootst= "<<RAD_TO_DEG(m0_temp-m1_temp)<<"(deg)"<<endl;


						if( ( distance_temp<50) )
						{
						vline.clear();
						}
						else
						{
							initialized=1;
							cout<<"Here 2"<<endl;
							float pole_diameter_inpx = distance_temp;
							//cout<<"prev = "<<pole_diameter_inpx<<" now = "<<pole_diameter_inpx_<<endl;

							//float pole_center = (out0.c+out1.c)/2;
							float pole_center = (vline[0].pt1.y+vline[1].pt1.y)/2;
							cout<<"line 0 y = "<<vline[0].pt1.y<<"line 1 y = "<<vline[1].pt1.y<<endl;
							//cout<<"prev ="<<pole_center<<" now = "<<pole_center_<<endl; 
							float init_x = (float)((focal_length*pole_diameter)/(pixel_size*pole_diameter_inpx));
							float init_y = (float)((pole_center-160)*pixel_size*init_x/focal_length);
							cout<<"Init x = "<<init_x<<endl;
							cout<<"Init y = "<<init_y<<endl;
							msg_pole_init.data.push_back(init_x);
							msg_pole_init.data.push_back(init_y);
							for(int i=0;i<10;i++) m_pose_init_pub.publish(msg_pole_init);
							msg_pole_init.data.clear();
							//ros::WallDuration(0.5).sleep();
							Vector2f pt0,pt1,pt2,pt3;
							pt0<<vline[0].pt0.x,vline[0].pt0.y,1;
							pt1<<vline[0].pt1.x,vline[0].pt1.y,1;
							pt2<<vline[1].pt0.x,vline[1].pt0.y,1;
							pt3<<vline[1].pt1.x,vline[1].pt1.y,1;

	
							boot_homline0=homline(pt0,pt1);
							boot_homline1=homline(pt2,pt3);

						}
						break;
					}
					else
					{
						//Firstly, pick up a line from candidates.
						
						line_t line_temp = vline[i+1];
						if( abs(line_base.pt0.x - line_temp.pt0.x)<10) 
						{
							//cout<<"erase element"<<endl;
							vline.erase(vline.begin()+i+1);
						}
					}
					//cout<<"vline.size()="<<vline.size()<<endl;
					
				
				}
			}

			#ifdef DRAW
			if(initialized)
			{
				for(int i =0;i<vline.size();i++) 
				{
					cvLine( draw_img, vline[i].pt0, vline[i].pt1, CV_RGB(255,255,0), 1, 8 );
				}
			}
			cvShowImage("Boot strapping", draw_img);
			#endif

		}//if(!initialized)


		//////////////////////////////////////
		//     After initialization
		//////////////////////////////////////
		
	    else
	    {

			//Generate two line models using Two-point form.

		    

			//for(int i=0;i<vsearch_u0.size();i++) cout<<vsearch_u0[i]<<endl;

			#ifdef CHECK_DURA
				t = (double)cv::getTickCount();
				total_begin_t=t;
			#endif

	

			/////////////////////////////////////
			//  Sobel masking
			/////////////////////////////////////

			//using the kernel of:
			//      [-1 -2 -1]
		    //      [ 0  0  0]
		    //      [ 1  2  1]
	        	Mat gray(grey);
			Sobel( gray, grad_x, CV_16S, 1, 0, 3, 1, 0, BORDER_DEFAULT );
			convertScaleAbs( grad_x, abs_grad_x );

			#ifdef SOBEL_Y
			Sobel( gray, grad_y, CV_16S, 0, 1, 3, 1, 0, BORDER_DEFAULT );
			convertScaleAbs( grad_y, abs_grad_y );

			addWeighted(abs_grad_x,0.9,abs_grad_y,0.5,0,grad);
		
			#ifdef DRAW
			cvtColor( grad, draw, CV_GRAY2BGR,3);
			#endif

			#else

			#ifdef DRAW
			cvtColor( abs_grad_x, draw, CV_GRAY2BGR,3);
			#endif

			#endif

			#ifdef CHECK_DURA
				double sobel_t = (((double)cv::getTickCount() - t)/cv::getTickFrequency())*1000;
				ROS_INFO("Sobel masking duration:\t %.3f ms", sobel_t);
			#endif

			//#ifdef CHECK_DURA
			//	track_t_begin = (double)cv::getTickCount();
			//#endif

			//m0 = (double)atan2(vline[0].pt1.y-vline[0].pt0.y,vline[0].pt1.x-vline[0].pt0.x);
			//m1 = (double)atan2(vline[1].pt1.y-vline[1].pt0.y,vline[1].pt1.x-vline[1].pt0.x);


			 #ifdef CHECK_DURA
				t = (double)cv::getTickCount();
			#endif


			//m0 = (double)atan2(vline[0].pt1.x-vline[0].pt0.x,vline[0].pt1.y-vline[0].pt0.y);
			//m1 = (double)atan2(vline[1].pt1.x-vline[1].pt0.x,vline[1].pt1.y-vline[1].pt0.y);
			

			m0 = (double)atan2(vline[0].pt1.y-vline[0].pt0.y,vline[0].pt1.x-vline[0].pt0.x);
			m1 = (double)atan2(vline[1].pt1.y-vline[1].pt0.y,vline[1].pt1.x-vline[1].pt0.x);
			
//			cout<<"init m0="<<m0<<" init m1="<<m1<<endl;
			
			for(int i =0;i<vsearch_v0.size();i++)
			{
					if(!bootstrapping)
					{
						//cout<<"bootstrapped"<<endl;
						//cout<<"m0 = "<<RAD_TO_DEG(m0)<<" m1= "<<RAD_TO_DEG(m1)<<endl;
						
						//vsearch_u0.push_back((float)(m0*(vsearch_v0[i]-vline[0].pt0.y)+vline[0].pt0.x));
						vsearch_u0.push_back((float)(-boot_homline0(1)*vsearch_v0[i]-boot_homline0(2))/boot_homline0(0));
		 				//cout<<"vsearch_u0="<<vsearch_u0[i]<<endl;
						vsearch_u1.push_back((float)(-boot_homline1(1)*vsearch_v1[i]-boot_homline1(2))/boot_homline1(0));

						//vsearch_u1.push_back((float)(-vsearch_v1[i]-vline[1].pt0.x)*m1+vline[1].pt0.y);
						//vsearch_u1.push_back((float)(m1*(vsearch_v1[i]-vline[1].pt0.y)+vline[1].pt0.x));
						//cout<<"vsearch_u1="<<vsearch_u1[i]<<endl;
					}
					
					CvPoint pt0,pt1;
					pt0.x=vsearch_u0[i];
					pt0.y=vsearch_v0[i];
					pt1.x=vsearch_u1[i];
					pt1.y=vsearch_v1[i];
			
					
					#ifdef DRAW
					circle(draw,pt0,1,CV_RGB(0,255,0),2,8,0);
					circle(draw,pt1,1,CV_RGB(0,255,255),2,8,0);
					#endif

#if 1

				    //cout<<grad<<endl;
					vector<unsigned char> vrow_px_proile0;
					vector<unsigned char> vrow_px_proile1;
	                //printf("grad=%d\n",grad.at<unsigned char>(100,100));
					for(int j=0;j<H_SEARCH_GAP*2;j++)
					{
						//cout<<"test22"<<endl;
						//cout<<"vsearch_v0[i]="<<vsearch_v0[i]<<" vsearch_u0[i] = "<<vsearch_u0[i]-H_SEARCH_GAP+j<<endl;
						//printf("abs_grad_x=%d\n",abs_grad_x.at<uchar>(vsearch_v0[i],vsearch_u0[i]-H_SEARCH_GAP+j));
						vrow_px_proile0.push_back(abs_grad_x.at<uchar>(vsearch_v0[i],vsearch_u0[i]-H_SEARCH_GAP+j));
						vrow_px_proile1.push_back(abs_grad_x.at<uchar>(vsearch_v1[i],vsearch_u1[i]-H_SEARCH_GAP+j));
					    vPixel_weight0 = vrow_px_proile0;
						vPixel_weight1 = vrow_px_proile1;
						
						//cout<<"test33"<<endl;
						//vvpixel_profile.push_back(vrow_px_proile);

					}

//cout<<"test44"<<endl;
					//for (int k=0;k<vrow_px_proile.size();k++) printf("%d ", vrow_px_proile[k]);
					//cout<<endl;
					int  idx0 = distance(vrow_px_proile0.begin(),max_element(vrow_px_proile0.begin(),vrow_px_proile0.end()));
					int  idx1 = distance(vrow_px_proile1.begin(),max_element(vrow_px_proile1.begin(),vrow_px_proile1.end()));
					//int value0 =*max_element(vrow_px_proile0.begin(),vrow_px_proile0.end());
					//cout<<"idx = "<<idx<<" value = "<<value<<endl;
					CvPoint pt_max0,pt_max1;
					Point2d pt_temp0,pt_temp1;
					
					pt_max0.x=vsearch_u0[i]-H_SEARCH_GAP+idx0;
					pt_max0.y=vsearch_v0[i];

					uf0.push_back((int)pt_max0.x);
					
					reg_uf0=uf0;
					reg_v0=vsearch_v0;
					
					pt_temp0.x = (double)pt_max0.x;
					pt_temp0.y = (double)pt_max0.y;

					
					
					pt_max1.x=vsearch_u1[i]-H_SEARCH_GAP+idx1;
					pt_max1.y=vsearch_v1[i];

					pt_temp1.x = (double)pt_max1.x;
					pt_temp1.y = (double)pt_max1.y;

					uf1.push_back((int)pt_max1.x);

					reg_uf1=uf1;
					reg_v1=vsearch_v1;

					vline0_candi.push_back(pt_temp0);
					vline1_candi.push_back(pt_temp1);
					//circle(draw,pt_max0,1,CV_RGB(255,0,255),2,8,0);
					//circle(draw,pt_max1,1,CV_RGB(255,0,0),2,8,0);

					//cout<<"vrow_px_proile.size()="<<vrow_px_proile.size()<<endl;
#endif
			}
			bootstrapping=1;

			#ifdef CHECK_DURA
				double search_t = (((double)cv::getTickCount() - t)/cv::getTickFrequency())*1000;
				ROS_INFO("Searching duration:\t %.3f ms", search_t);
			#endif

			//cout<<"vline0_candi.size="<<vline0_candi.size()<<endl;

			//model= LinearModel(m, b);
			//model.mSlope=m0;
#if 1
		    #ifdef CHECK_DURA
				t = (double)cv::getTickCount();
			#endif
			Ransac0.setObservationSet(vline0_candi);
			if(Ransac0.getBestModel(model))
			{
				//cout<<"m="<<model.mSlope<<" c="<<model.mIntercept<<" # inliers="<<Ransac0.mBestConsensusSet.size()<<endl;
			    //cout<<"mBestConsensusSet0.size()="<<Ransac0.mBestConsensusSet.size()<<endl;
				//cout<<"inlier.x="<<Ransac0.mBestConsensusSet[0].x<<"  inlier.y="<<Ransac0.mBestConsensusSet[0].y<<endl;
				#ifdef DRAW
				for(int i=0;i<Ransac0.mBestConsensusSet.size();i++)
				{
					circle(draw,Ransac0.mBestConsensusSet[i],1,CV_RGB(255,0,255),2,8,0);
				}
				#endif

#if 0
				for(int i=0;i<Ransac0.mOutLiers.size();i++)
				{
					circle(draw,Ransac0.mOutLiers[i],1,CV_RGB(255,255,0),2,8,0);
				}
#endif
				//line0_x = (int)(model.mIntercept/cos(model.mSlope));
				//cout<<"Pole left x="<<line0_x<<endl;
			}

			#if 1
	        	Ransac1.setObservationSet(vline1_candi);
			if(Ransac1.getBestModel(model))
			{
				//cout<<"m="<<model.mSlope<<" c="<<model.mIntercept<<"# inliers="<<Ransac1.mBestConsensusSet.size()<<endl;
			    //cout<<"mBestConsensusSet1.size()="<<Ransac1.mBestConsensusSet.size()<<endl;
				#ifdef DRAW
				for(int i=0;i<Ransac1.mBestConsensusSet.size();i++)
				{
					circle(draw,Ransac1.mBestConsensusSet[i],1,CV_RGB(255,0,0),2,8,0);
				}
				#endif
#if 0
				for(int i=0;i<Ransac1.mOutLiers.size();i++)
				{
					circle(draw,Ransac1.mOutLiers[i],1,CV_RGB(255,255,0),2,8,0);
				}
#endif	
				//line1_x = (int)(-model.mIntercept/model.mSlope);
				//cout<<"Pole left x="<<line1_x<<endl;
			}
			#endif

			#ifdef CHECK_DURA
				double ransac_t = (((double)cv::getTickCount() - t)/cv::getTickFrequency())*1000;
				ROS_INFO("RANSAC duration:\t %.3f ms", ransac_t);
			#endif
			

#if 0
			Ransac0.run();  

			cout<<"mBestConsensusSet0.size()="<<Ransac0.mBestConsensusSet.size()<<endl;
			for(int i=0;i<Ransac0.mBestConsensusSet.size();i++)
			{
				circle(draw,Ransac0.mBestConsensusSet[i],1,CV_RGB(255,0,255),2,8,0);
			}
			
			Ransac1.setObservationSet(vline1_candi);
			Ransac1.run();
			cout<<"mBestConsensusSet1.size()="<<Ransac1.mBestConsensusSet.size()<<endl;
			for(int i=0;i<Ransac1.mBestConsensusSet.size();i++)
			{
				circle(draw,Ransac1.mBestConsensusSet[i],1,CV_RGB(255,0,0),2,8,0);
			}
#endif



			#ifdef CHECK_DURA
				t = (double)cv::getTickCount();
			#endif

			//////////////////////////////////
			//     Regression
			//////////////////////////////////
			vector<float> resid0,resid1;

#ifdef NOMAL_REGRESSION
			while(1)
			{
				//////////////////////////////
				//Regression for line1
				//////////////////////////////
				
				return_t ret0;
				ret0=regression(reg_uf0,reg_v0);

				// compute the residual magnitude for each point
				for(int i=0;i<reg_uf0.size();i++)
				{
					resid0.push_back((float)abs(reg_v0[i]*ret0.m+ret0.c - reg_uf0[i]));
				}
				//cout<<"resid0.size()="<<resid0.size()<<endl;

				//for(int i=0;i<resid0.size();i++) cout<<"resid0["<<i<<"]="<<resid0[i]<<endl;

				//printf("max resid0 =%f \n",*max_element(resid0.begin(),resid0.end()));
				// if the resid is low then quit
	            if( *max_element(resid0.begin(),resid0.end())< REGRESSION_THRESHOLD )
	            {
		            //cout<<"in threshold line0"<<endl;
		            out0=ret0;
					resid0.clear();
		            break;
	            }
				
				else
				{
					//cout<<"get rid of element which has the biggest error from resid0"<<endl;
					int  idx0 = distance(resid0.begin(),max_element(resid0.begin(),resid0.end()));
					//cout<<"max resid0 = "<<resid0[idx0]<<" idx0 = "<<idx0<<endl;
					reg_v0.erase(reg_v0.begin()+idx0);
					reg_uf0.erase(reg_uf0.begin()+idx0);
					resid0.clear();
					//resid0.erase(idx0);
					//int  idx1 = distance(vrow_px_proile1.begin(),max_element(vrow_px_proile1.begin(),vrow_px_proile1.end()));
				}
			} //while(1) for line0



			while(1)
			{
				//////////////////////////////
				//Regression for line1
				//////////////////////////////
				return_t ret1;
				ret1=regression(reg_uf1,reg_v1);

				// compute the residual magnitude for each point
				for(int i=0;i<reg_uf1.size();i++)
				{
					resid1.push_back((float)abs(reg_v1[i]*ret1.m+ret1.c - reg_uf1[i]));
				}
				//cout<<"resid1.size()="<<resid1.size()<<endl;

				//for(int i=0;i<resid1.size();i++) cout<<"resid1["<<i<<"]="<<resid1[i]<<endl;

				//printf("max resid1 =%f \n",*max_element(resid1.begin(),resid1.end()));
				// if the resid is low then quit
	            if( *max_element(resid1.begin(),resid1.end())< REGRESSION_THRESHOLD )
	            {
		            //cout<<"in threshold line 1"<<endl;
		            out1=ret1;
					resid1.clear();
		            break;
	            }
				
				else
				{
					//cout<<"get rid of element which has the biggest error"<<endl;
					int  idx1 = distance(resid1.begin(),max_element(resid1.begin(),resid1.end()));
					//cout<<"max resid1 = "<<resid1[idx1]<<" idx1 = "<<idx1<<endl;
					reg_v1.erase(reg_v1.begin()+idx1);
					reg_uf1.erase(reg_uf1.begin()+idx1);
					resid1.clear();
					//resid0.erase(idx0);
					//int  idx1 = distance(vrow_px_proile1.begin(),max_element(vrow_px_proile1.begin(),vrow_px_proile1.end()));
				}
			}
#endif

#ifdef WTLS
			return_t out0 = wtls(vsearch_v0,uf0,vPixel_weight0);
			return_t out1 = wtls(vsearch_v1,uf1,vPixel_weight1);
#endif

#ifdef RANSAC_MODEL
		    	return_t out0;
			return_t out1;
			out0.m = (float)Ransac0.mBestModel.mSlope;
			out0.c = (float)Ransac0.mBestModel.mIntercept;

			out1.m = (float)Ransac1.mBestModel.mSlope;
			out1.c = (float)Ransac1.mBestModel.mIntercept;
#endif

			/////////////////////////////////////////////
			// Publishing pole line detection for EKF
			/////////////////////////////////////////////

			msg_pole_out.data.push_back(-out0.m);
			msg_pole_out.data.push_back(out0.c);
			msg_pole_out.data.push_back(-out1.m);
			msg_pole_out.data.push_back(out1.c);
			m_pole_dec_pub.publish(msg_pole_out);
			



			//////////////////////////////
			// Update searching area for next iteration based on 
			// current line model (m,c)
			//////////////////////////////
			
			//cout<<"m0= "<<out0.m<<" c0= "<<out0.c<<endl;
			vsearch_v0.clear();
			vsearch_v0=vsearch_v0_pre;
			vsearch_u0.clear();
			for(int i=0;i<vsearch_v0.size();i++)
			{
				float u0=out0.m*vsearch_v0[i]+out0.c;
				vsearch_u0.push_back((int)u0); 
			}

			//cout<<"m1= "<<out1.m<<" c1= "<<out1.c<<endl;
			vsearch_v1.clear();
			vsearch_v1=vsearch_v1_pre;
			vsearch_u1.clear();
			for(int i=0;i<vsearch_v1.size();i++)
			{
				float u1=out1.m*vsearch_v1[i]+out1.c;
				vsearch_u1.push_back((int)u1); 
			}
			current_time=ros::Time::now();

			

			//float l = abs(out0.c-out1.c);
			
			//float pole_diameter_inpx = l*cos((float)((out0.m+out1.m)/2));
			float v0 = out0.m*pu+out0.c;
			float v1 = out1.m*pu+out1.c;
	 		float pole_diameter_inpx = abs( v0-v1);
			//cout<<"prev = "<<pole_diameter_inpx<<" now = "<<pole_diameter_inpx_<<endl;
			
			//float pole_center = (out0.c+out1.c)/2;
			float pole_center = (v0+v1)/2;
			//cout<<"prev ="<<pole_center<<" now = "<<pole_center_<<endl; 
			float dx = (float)((focal_length*pole_diameter)/(pixel_size*pole_diameter_inpx));
			float dy = (float)((pole_center-160)*pixel_size*dx/focal_length);
			//cout<<"y = "<<dy<<endl;

			#ifdef KALMAN
			Vector4f esti;
			Matrix < float, 2, 1 > measure;
			measure(0)=dx;
			measure(1)=dy;

			esti=kalman.process(measure);

			m_pole_pose.x=esti(0);
			m_pole_pose.y=esti(2);

			#else
			m_pole_pose.x=dx;
			m_pole_pose.y=dy;

			#endif

			m_pose_pub.publish(m_pole_pose);

			
			//fprintf('Left line m= %f c= %f\n', m2,c2);
			//fprintf('Right line m= %f c= %f\n', m1,c1);
			//fprintf('Pseudo distance btw two line = %f\n', pole_diameter_inpx);
			//fprintf('distance x= %f metre distance y=%f metre\n',dx,dy);
			fprintf(fd,"%f,%f,%f,%f,%f,%f,%f,\n",out0.m,out0.c,out1.m,out1.c,m_pole_pose.x,m_pole_pose.y,current_time.toSec());

			#ifdef CHECK_DURA
				double regression_t = (((double)cv::getTickCount() - t)/cv::getTickFrequency())*1000;
				ROS_INFO("Regression duration:\t %.3f ms", regression_t);
			#endif


			//#ifdef CHECK_DURA
			//	double track_t = (((double)cv::getTickCount() - track_t_begin)/cv::getTickFrequency())*1000;
			//	ROS_INFO("Traking duration:\t %.3f ms", track_t);
			//#endif


			


			///pt.x=vsearch_u0[i];
			//		pt.y=vsearch_v0[i];
			
			
			
			

			

		    #if 0
		 	% create the new set of centre points
	        vsearch1 = vsearch0_1;
	        vsearch2 = vsearch0_2;
	        usearch1 = m1*vsearch1+c1;
	        usearch2 = m2*vsearch2+c2;
			#endif
	   
				#if 0
						
            resid1 = abs(vsearch1*m1+c1 - uf1')'
            resid2 = abs(vsearch2*m2+c2 - uf2')'

            % if the resid is low then quit
            if max(resid1) < 0.3
                break;
            end

            if max(resid2) < 0.3
                break;
            end
			
			}
			#endif
			
			
		    //Ransac.setModel(model);
		    //if()
		    //{
			    //cout<<"!!!!!!!model.slope= "<<model.mSlope<<" model.inter="<<model.mIntercept<<endl;
		        //cvLine( draw, vline[i].pt0, vline[i].pt1, CV_RGB(255,255,0), 1, 8 );
			//}

			

			
			
			//cout<<"vvpixel_profile.size()="<<vvpixel_profile.size()<<endl;
#endif



		    #ifdef DRAW
		    imshow(window_name,draw);
		    #endif

		} //else (initialized)

		

		
			
		//for(int i=0;i<vline_length.size();i++) cout<<vline_length[i]<<endl;
		
		// see how much time has elapsed
		//time(&end);

		// calculate current FPS
		//++counter;		
		//sec = difftime (end, app_start);      

		//fps = counter / sec;


		//total = (((double)cv::getTickCount() - total)/cv::getTickFrequency())*1000;	
		//printf("FPS = %.2f Duration = %4.2fms \n", fps, t);
		//sprintf(text,"Duration : %4.2fms fps : %4.2f", total,1/total*1000);
		//sprintf(text,"Duration : %4.2fms", total);

		//IplImage* img = new IplImage(frame);
		//cvPutText(draw_img,text,cvPoint(20,20),&font,cvScalar(255,255,255));

		//IplImage draw(grad);
		//cout<<"test333333333"<<endl;
		/* show loaded frame */
		//cvShowImage(window_name, draw);

		
	#ifdef CHECK_DURA

		double total_t = (((double)cv::getTickCount() - total_begin_t)/cv::getTickFrequency())*1000;
		ROS_INFO("Total duration:\t %.3f ms", total_t);
	
		//fprintf(fd_profile,"Preperation,canny,Hough_transform,Visualization,total,time\n");
		current_time = ros::Time::now();
		//fprintf(fd_profile,"%f,%f,%f,%f,%f,%f\n",pre,canny_t,hough_t,visual_t,total,current_time.toSec());
	#endif

		/* wait delay and check for the quit key */
		key = cvWaitKey(1);
		//if(key=='q') ros::shutdown();
		#ifdef DRAW
		cvReleaseImage(&draw_img);
		#endif

		cvReleaseImage(&grey);
		cvReleaseImage(&canny_edges);
		vline_length.clear();
		vline.clear();
		vvpixel_profile.clear();
		vline0_candi.clear();
		vline1_candi.clear();
		Ransac0.mBestConsensusSet.clear();
		Ransac0.mObservationSet.clear();
		Ransac0.mBestError=INT_MAX;
		Ransac0.mOutLiers.clear();


		Ransac1.mBestConsensusSet.clear();
		Ransac1.mObservationSet.clear();
		Ransac1.mBestError=INT_MAX;
		Ransac1.mOutLiers.clear();
		uf0.clear();
		uf1.clear();
		reg_uf0.clear();
		reg_uf1.clear();
		reg_v0.clear();
		reg_v1.clear();
		vPixel_weight0.clear();
		vPixel_weight1.clear();

		msg_pole_out.data.clear();
		
		//cout<<"\n\n"<<endl;f
		
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "pole_vision");
	ros::NodeHandle nh;
	//fd_profile = fopen("profile","w");
	//fprintf(fd_profile,"Preperation,canny,Hough_transform,Visualization,total,time\n");
	fd=fopen("linemodel.data","w");
	fprintf(fd,"m0,c0,m1,c1,x,y,time\n");
	storage = cvCreateMemStorage(0);
	lines = 0;
	int j;

	//model= LinearModel(m, b);
	Ransac0.setIterations(RANSAC_ITERATION);
	Ransac0.setRequiredInliers(RANSAC_NUM_REQUIRE); // 5 out of 24
	Ransac0.setTreshold(RANSAC_THRESHOLD);  // 2 pixels distance

	Ransac1.setIterations(RANSAC_ITERATION);
	Ransac1.setRequiredInliers(RANSAC_NUM_REQUIRE); // 5 out of 24
	Ransac1.setTreshold(RANSAC_THRESHOLD);  // 2 pixels distance


	for(int i=0;i<(IMG_HEIGHT/V_GAP);i++)
	{
		vsearch_v0.push_back(i*V_GAP);
		vsearch_v1.push_back(i*V_GAP);
	}	

	//for(int i=0;i<vsearch_v0.size();i++) cout<<vsearch_v0[i]<<endl;

	vsearch_v0_pre = vsearch_v0;
	vsearch_v1_pre = vsearch_v1;

	#ifdef KALMAN
	kalman.init();
	#endif

	

	
	

	//for(int i =0;i<vsearch_v0.size();i++) cout<<vsearch_v0[i]<<vsearch_v1[i]<<endl;
		

	

	// start the clock
	//time(&app_start);

	//cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.5,0.5,0,2,8);


	//cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, IMG_WIDTH);
	//cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, IMG_HEIGHT);

	// get a frame size to be used by writer structure
	//CvSize size = cvSize (IMG_WIDTH,IMG_HEIGHT);
	image_transport::ImageTransport it(nh);
	//image_transport::Subscriber sub = it.subscribe("/image_raw", 1, imageCallback);
	image_transport::Subscriber sub = it.subscribe("/fwd/image_raw", 1, imageCallback);
	//ros::Rate loop_rate(100);	

	#ifdef DRAW
	/* create a video window with same name of the video file, auto sized */
	window_name = "Fast camera view";
	cvNamedWindow(window_name, CV_WINDOW_AUTOSIZE);
	cvNamedWindow("Boot strapping", CV_WINDOW_AUTOSIZE);

#ifdef TRACKBAR
	cvCreateTrackbar("Threshold1",window_name,&nThreshold1,5000,NULL);
	cvCreateTrackbar("Threshold2",window_name,&nThreshold2,5000,NULL);
	cvCreateTrackbar("Aperture",window_name,&nAperture,7,NULL);
	cvCreateTrackbar("Threshold for hough",window_name,&nthreshold_hough,200,NULL);
	cvCreateTrackbar("Param1 for hough",window_name,&nparam1_hough,100,NULL);
	cvCreateTrackbar("Param2 for hough",window_name,&nparam2_hough,100,NULL);
	
#endif
	cvStartWindowThread();

	#endif


	ros::Rate loop_rate(LOOP_HZ);	// 5HZ
	m_pose_pub  = nh.advertise<geometry_msgs::Pose2D>("vision_pole_pose", 1);
	m_pole_dec_pub = nh.advertise<std_msgs::Float32MultiArray>("pole_dec_out", 1);
	m_pose_init_pub = nh.advertise<std_msgs::Float32MultiArray>("PolePoseInit", 1);

	//ros::spin();
	while (ros::ok())
	//while (1)
	{
		ros::spinOnce();	
		/* wait delay and check for the quit key */
		//char key = cvWaitKey(5);
		//if(key=='q') break;
		loop_rate.sleep();	
		//usleep(1000); //sleep for 10ms
	} //while

	fclose(fd);
	#ifdef DRAW
	cvDestroyWindow(window_name);
	#endif
}

return_t regression(vector<int> y,vector<int> x)
{
	 vector<int> sx;
	 vector<int> sy;
	 vector<int> sx2;
	 vector<int> sy2;
	 vector<int> sxy;
	 
	 return_t ret;
	 int n= x.size();

	 for(int i=0;i<n;i++)
	 {
		 //cout<<"x="<<x[i]<<" y="<<y[i]<<endl;
		 sx.push_back(x[i]);
		 sy.push_back(y[i]);
		 sx2.push_back(x[i]*x[i]);
		 sy2.push_back(y[i]*y[i]);
		 sxy.push_back(x[i]*y[i]);
	}
	 //cout<<"size of sx="<<sx.size()<<endl;
     int sx_=accumulate(sx.begin(),sx.end(),0.0f);
	 int sy_=accumulate(sy.begin(),sy.end(),0.0f);
	 int sx2_=accumulate(sx2.begin(),sx2.end(),0.0f);
	 int sy2_=accumulate(sy2.begin(),sy2.end(),0.0f);
	 int sxy_=accumulate(sxy.begin(),sxy.end(),0.0f);

	 float denom = n*sx2_ - sx_*sx_;

	ret.m = (float)((n*sxy_ - sx_*sy_)/denom);
	ret.c = (float)((sx2_*sy_ - sx_*sxy_)/denom);

	//cout<<"ret.m = "<<ret.m<<" ret.c="<<ret.c<<endl;
#if 0
 //avg=accumulate(sum.begin(),sum.end(),0.0f)/sum.size();
	 sx = sum(x);
	sy = sum(y);
	sx2 = sum(x.*x);
	sy2 = sum(y.*y);
	sxy = sum(x.*y);
	n = length(x);

	denom = n*sx2 - sx*sx;

	m = (n*sxy - sx*sy)/denom;
	c = (sx2*sy - sx*sxy)/denom;

#endif
	
	 return ret;
}


return_t wtls(vector<int> x,vector<int> y,vector<unsigned char> w)
{
	 return_t ret;
	 vector<float> x_,y_;
	 int n=x.size();

	// cout<<"w="<<Mat(w)<<endl;

	// for(int i=0;i<w.size();i++) cout<<"w="<<w[i]<<endl;

	 
	 float xm = (float)(accumulate(x.begin(),x.end(),0.0f)/x.size());
	 float ym = (float)(accumulate(y.begin(),y.end(),0.0f)/y.size());
	 //cout<<"xm="<<xm<<"ym="<<ym<<endl;
	 for(int i=0;i<n;i++) 
	 {
		 //cout<<"x="<<x[i]<<endl;
		 //cout<<"y="<<y[i]<<endl;
		 
		 float temp_x = (float)(x[i]-xm);
		 float temp_y = (float)(y[i]-ym);
		 x_.push_back(temp_x);
		 y_.push_back(temp_y);

		 //cout<<"x_="<<x_[i]<<endl;
		 //cout<<"y_="<<y_[i]<<endl;
	 }
	 //cout<<"x_="<<Mat(x_)<<endl;
	 //cout<<"y_="<<Mat(y_)<<endl;
	 

	 

	 Mat M = Mat::zeros(2,2,CV_32F);

	 Mat xy= Mat::zeros(n,3,CV_32F);

	 Mat p=Mat::zeros(n,2,CV_32F);
	 //Mat p=Mat(0,2,CV_32F);
	 
	 for(int i=0;i<n;i++)
	 {
		 xy.at<float>(i,0) = x_[i];
		 xy.at<float>(i,1) = y_[i];
	 }
	 //cout<<"xy="<<xy<<endl;
	 
	 /////////////////////////////////
	 // Where you want to copy data
	 /////////////////////////////////
	 Mat p0=p.col(0);
	 Mat p1=p.col(1);

	 ////////////////////////////////////////////////
	 // Copy data from xy to where you point above
	 ////////////////////////////////////////////////
	 xy.col(0).copyTo(p0);
	 xy.col(1).copyTo(p1);
	 
	 //cout<<"p="<<p<<endl;
	 //cout<<"after xy="<<xy<<endl;
	 
	 Mat p_ = Mat(1,2,CV_32F);
	 
	 for(int i=0;i<n;i++)
	 {
		 p.row(i).copyTo(p_);
		 //cout<<"p_="<<p_<<endl;
		 M = M+p_.t()*p_;
		 //cout<<"M="<<M<<endl;
	 }
	 Mat U,S,V;
	 //Mat R= (Mat_<float>(2,2)<< 10,10,15,15);
	 //cout<<"R="<<R<<endl;
	 cv::SVD::compute(M,S,U,V);

	 cout<<"U="<<U<<endl;
	 cout<<"S="<<S<<endl;
	 cout<<"V="<<V<<endl;

	 Mat normal=Mat(2,1,CV_32F);
	 U.col(1).copyTo(normal);
	 cout<<"normal="<<normal<<endl;

	 Mat mean= (Mat_<float>(2,1)<< xm,ym);

	 //cout<<"R="<<R<<endl;

	 Mat distance = normal.t()*mean;

	 cout<<"distance="<<distance<<endl;
	 float c=distance.at<float>(0,0);
	 float m=-normal.at<float>(0,0);
	 cout<<"c="<<c<<endl;
	 cout<<"m="<<m<<endl;

	 ret.c=c;
	 ret.m=m;
	 

	 
	 
	
	 return ret;
}

double getDistance_Pt_Line(CvPoint& P, CvPoint& A, CvPoint& B)
{

	double distance = INT_MAX;
	double normalLength= sqrt( (B.x-A.x)*(B.x-A.x)+(B.y-A.y)*(B.y-A.y));
	double v = abs( (P.x-A.x)*(B.y-A.y) - (P.y-A.y)*(B.x-A.x));
	distance = (double) (v/normalLength);
	//cout<<"distance="<<distance<<endl;
	return distance;
}

Vector3f homline(Vector2f A, Vector2f B)
{
	Vector3f pt0,pt1,pt2,pt3;
	pt0<<A,1;
	pt1<<B,1;
	Vector3f line0 = pt0.cross(pt1);
	//cout<<"line 0 = "<<line0<<endl;
	Vector2f norm;
	norm(0)=line0(0);
	norm(1)=line0(1);
	//cout<<"norm = "<<norm.norm()<<endl;
	//cout<<"norm l0= "<<line0/norm.norm()<<endl;
	return line0/norm.norm();
}




